In [1]:

    

from __future__ import print_function
import sympy
import sympy.physics.mechanics as mech
sympy.init_printing()
mech.init_vprinting()


    

In [2]:

    

t = sympy.symbols('t')
rot_N, rot_E, rot_D, vel_N, vel_E, vel_D, \
    gyro_bias_N, gyro_bias_E, gyro_bias_D, \
    accel_bias_N, accel_bias_E, accel_bias_D, \
    pos_N, pos_E, asl, terrain_asl, baro_bias, \
    wind_N, wind_E, wind_D, d, agl, phi, theta, psi  = mech.dynamicsymbols(
    'rot_N, rot_E, rot_D, vel_N, vel_E, vel_D, ' \
    'gyro_bias_N, gyro_bias_E, gyro_bias_D, ' \
    'accel_bias_N, accel_bias_E, accel_bias_D, ' \
    'pos_N, pos_E, asl, terrain_asl, baro_bias, ' \
    'wind_N, wind_E, wind_D, d, agl, phi, theta, psi') 
frame_i = mech.ReferenceFrame('i')
frame_n = frame_i.orientnew('n', 'Quaternion', (1, rot_N, rot_E, rot_D))
#frame_b = frame_n.orientnew('b', 'Quaternion', (q_0, q_1, q_2, q_3)) 
# easier to see where we get divide by zeros if we express dcm in euler angles
frame_b = frame_n.orientnew('b', 'Body', (psi, theta, phi), '321')
C_nb = frame_n.dcm(frame_b)
assert C_nb[0, 1] == frame_n.x.dot(frame_b.y)
sub_C_nb = {}
for i in range(3):
    for j in range(3):
        sub_C_nb[C_nb[i, j]] = sympy.Symbol('C_nb({:d}, {:d})'.format(i, j))(t)
        sub_C_nb[-C_nb[i, j]] = -sympy.Symbol('C_nb({:d}, {:d})'.format(i, j))(t)
sub_C_nb_rev = { sub_C_nb[key]: key for key in sub_C_nb.keys() }
sub_lin = {
    rot_N: 0,
    rot_E: 0,
    rot_D: 0,
    gyro_bias_N: 0,
    gyro_bias_E: 0,
    gyro_bias_D: 0
}
sub_agl = {
    asl - terrain_asl: agl
}
omega_bx, omega_by, omega_bz = mech.dynamicsymbols('omega_bx, omega_by, omega_bz')
flowX, flowY = mech.dynamicsymbols('flowX, flowY')
omega_ib_b = omega_bx * frame_b.x \
    + omega_by * frame_b.y \
    + omega_bz * frame_b.z
gyro_bias_i = gyro_bias_N * frame_i.x \
    + gyro_bias_E * frame_i.y \
    + gyro_bias_D * frame_i.z
omega_nx, omega_ny, omega_nz = mech.dynamicsymbols('omega_nx, omega_ny, omega_nz')
omega_in_n = -gyro_bias_N * frame_n.x \
    - gyro_bias_E * frame_n.y \
    - gyro_bias_D * frame_n.z
a_N, a_E, a_D = mech.dynamicsymbols('a_N, a_E, a_D')
a_n = a_N*frame_n.x + a_E*frame_n.y + a_D*frame_n.z
a_bias_n = accel_bias_N*frame_n.x + accel_bias_E*frame_n.y + accel_bias_D*frame_n.z
a_n_correct = a_n - a_bias_n
v_i = vel_N*frame_i.x + vel_E*frame_i.y + vel_D*frame_i.z
p_i = pos_N*frame_i.x + pos_E*frame_i.y - asl*frame_i.z
I_wx, I_wy, I_wz = mech.dynamicsymbols('I_wx, I_wy, I_wz')
I_w_n = I_wx*frame_n.x + I_wy*frame_n.y  + I_wz*frame_n.z


    

In [3]:

    

xe = sympy.Matrix([rot_N, rot_E, rot_D, vel_N, vel_E, vel_D, gyro_bias_N, gyro_bias_E, gyro_bias_D,
                   accel_bias_N, accel_bias_E, accel_bias_D,
                   pos_N, pos_E, asl, terrain_asl, baro_bias, wind_N, wind_E, wind_D])
xe.T


    

    
Out[3]:





$$\left[\begin{array}{cccccccccccccccccccc}rot_{N} & rot_{E} & rot_{D} & vel_{N} & vel_{E} & vel_{D} & gyro_{biasN} & gyro_{biasE} & gyro_{biasD} & accel_{biasN} & accel_{biasE} & accel_{biasD} & pos_{N} & pos_{E} & asl & terrain_{asl} & baro_{bias} & wind_{N} & wind_{E} & wind_{D}\end{array}\right]$$

In [4]:

    

def print_terms(terms):
    for t in terms:
        s = 'float {:s} = {:s};'.format(
                str(t[0]), str(t[1]))
        print(s.replace('(t)', ''))
        
def matrix_to_code(name, mat, i_name, i_syms, j_name, j_syms):
    print('Matrix<float, {:s}n, {:s}n> {:s};'.format(i_name, j_name, name))
    mat.simplify()
    terms, mat = sympy.cse(mat)
    print_terms(terms)
    mat = mat[0]
    for i in range(mat.shape[0]):
        for j in range(mat.shape[1]):
            if str(mat[i, j]) == "0":
                continue
            s = '{:s}({:s}{:s}, {:s}{:s}) = {:s};'.format(
                str(name), i_name, str(i_syms[i]),
                j_name, str(j_syms[j]), str(mat[i, j]))
            print(s.replace('(t)', ''))


    

In [5]:

    

trans_kin_eqs = list((a_n_correct.express(frame_i) - v_i.diff(t, frame_i)).to_matrix(frame_i))
trans_kin_eqs


    

    
Out[5]:





$$\left [ \left(2 rot_{D} rot_{N} + 2 rot_{E}\right) \left(a_{D} - accel_{biasD}\right) + \left(a_{E} - accel_{biasE}\right) \left(- 2 rot_{D} + 2 rot_{E} rot_{N}\right) + \left(a_{N} - accel_{biasN}\right) \left(- rot^{2}_{D} - rot^{2}_{E} + rot^{2}_{N} + 1\right) - \dot{vel}_{N}, \quad \left(2 rot_{D} rot_{E} - 2 rot_{N}\right) \left(a_{D} - accel_{biasD}\right) + \left(a_{E} - accel_{biasE}\right) \left(- rot^{2}_{D} + rot^{2}_{E} - rot^{2}_{N} + 1\right) + \left(a_{N} - accel_{biasN}\right) \left(2 rot_{D} + 2 rot_{E} rot_{N}\right) - \dot{vel}_{E}, \quad \left(2 rot_{D} rot_{E} + 2 rot_{N}\right) \left(a_{E} - accel_{biasE}\right) + \left(2 rot_{D} rot_{N} - 2 rot_{E}\right) \left(a_{N} - accel_{biasN}\right) + \left(a_{D} - accel_{biasD}\right) \left(rot^{2}_{D} - rot^{2}_{E} - rot^{2}_{N} + 1\right) - \dot{vel}_{D}\right ]$$

In [6]:

    

nav_eqs = list((p_i.diff(t, frame_i) - v_i).to_matrix(frame_i))
nav_eqs


    

    
Out[6]:





$$\left [ - vel_{N} + \dot{pos}_{N}, \quad - vel_{E} + \dot{pos}_{E}, \quad - vel_{D} - \dot{asl}\right ]$$

In [7]:

    

sub_q = {
    (1 + rot_N**2 + rot_E**2 + rot_D**2): 1,
    2*(1 + rot_N**2 + rot_E**2 + rot_D**2): 2
}


    

In [8]:

    

rot_kin_eqs = list((frame_n.ang_vel_in(frame_i) - omega_in_n).to_matrix(frame_n))
rot_kin_eqs


    

    
Out[8]:





$$\left [ gyro_{biasN} + 2 rot_{D} \dot{rot}_{E} - 2 rot_{E} \dot{rot}_{D} + 2 \dot{rot}_{N}, \quad gyro_{biasE} - 2 rot_{D} \dot{rot}_{N} + 2 rot_{N} \dot{rot}_{D} + 2 \dot{rot}_{E}, \quad gyro_{biasD} + 2 rot_{E} \dot{rot}_{N} - 2 rot_{N} \dot{rot}_{E} + 2 \dot{rot}_{D}\right ]$$

In [9]:

    

static_eqs = [
    terrain_asl.diff(t),
    baro_bias.diff(t),
    wind_N.diff(t),
    wind_E.diff(t),
    wind_D.diff(t),
    accel_bias_N.diff(t),
    accel_bias_E.diff(t),
    accel_bias_D.diff(t),
]
static_eqs


    

    
Out[9]:





$$\left [ \dot{terrain}_{asl}, \quad \dot{baro}_{bias}, \quad \dot{wind}_{N}, \quad \dot{wind}_{E}, \quad \dot{wind}_{D}, \quad \dot{accel}_{biasN}, \quad \dot{accel}_{biasE}, \quad \dot{accel}_{biasD}\right ]$$

In [10]:

    

static_eqs


    

    
Out[10]:





$$\left [ \dot{terrain}_{asl}, \quad \dot{baro}_{bias}, \quad \dot{wind}_{N}, \quad \dot{wind}_{E}, \quad \dot{wind}_{D}, \quad \dot{accel}_{biasN}, \quad \dot{accel}_{biasE}, \quad \dot{accel}_{biasD}\right ]$$

In [11]:

    

gyro_eqs = list((omega_in_n.diff(t, frame_n) - frame_i.ang_vel_in(frame_n).cross(I_w_n)).to_matrix(frame_n))
gyro_eqs


    

    
Out[11]:





$$\left [ - \left(2 rot_{D} \dot{rot}_{N} - 2 rot_{N} \dot{rot}_{D} - 2 \dot{rot}_{E}\right) I_{wz} + \left(- 2 rot_{E} \dot{rot}_{N} + 2 rot_{N} \dot{rot}_{E} - 2 \dot{rot}_{D}\right) I_{wy} - \dot{gyro}_{biasN}, \quad \left(- 2 rot_{D} \dot{rot}_{E} + 2 rot_{E} \dot{rot}_{D} - 2 \dot{rot}_{N}\right) I_{wz} - \left(- 2 rot_{E} \dot{rot}_{N} + 2 rot_{N} \dot{rot}_{E} - 2 \dot{rot}_{D}\right) I_{wx} - \dot{gyro}_{biasE}, \quad - \left(- 2 rot_{D} \dot{rot}_{E} + 2 rot_{E} \dot{rot}_{D} - 2 \dot{rot}_{N}\right) I_{wy} + \left(2 rot_{D} \dot{rot}_{N} - 2 rot_{N} \dot{rot}_{D} - 2 \dot{rot}_{E}\right) I_{wx} - \dot{gyro}_{biasD}\right ]$$

In [12]:

    

sol = sympy.solve(rot_kin_eqs + trans_kin_eqs + static_eqs + nav_eqs + gyro_eqs, xe.diff(t))
sol = { key:sol[key].subs(sub_q) for key in sol.keys() }
xe_dot = sympy.Matrix([ sol[var] for var in xe.diff(t) ]).applyfunc(lambda x: x.subs(sub_q))
#xe_dot


    

In [13]:

    

A = xe_dot.jacobian(xe).subs(sub_lin)
#A


    

In [14]:

    

matrix_to_code('A', A, 'Xe::', xe, 'Xe::', xe)


    

    




Matrix<float, Xe::n, Xe::n> A;
float x0 = 2*a_D;
float x1 = 2*accel_bias_D;
float x2 = 2*a_E;
float x3 = 2*accel_bias_E;
float x4 = 2*a_N;
float x5 = 2*accel_bias_N;
float x6 = I_wz;
float x7 = I_wy;
float x8 = I_wx;
A(Xe::rot_N, Xe::gyro_bias_N) = -1/2;
A(Xe::rot_E, Xe::gyro_bias_E) = -1/2;
A(Xe::rot_D, Xe::gyro_bias_D) = -1/2;
A(Xe::vel_N, Xe::rot_E) = x0 - x1;
A(Xe::vel_N, Xe::rot_D) = -x2 + x3;
A(Xe::vel_N, Xe::accel_bias_N) = -1;
A(Xe::vel_E, Xe::rot_N) = -x0 + x1;
A(Xe::vel_E, Xe::rot_D) = x4 - x5;
A(Xe::vel_E, Xe::accel_bias_E) = -1;
A(Xe::vel_D, Xe::rot_N) = x2 - x3;
A(Xe::vel_D, Xe::rot_E) = -x4 + x5;
A(Xe::vel_D, Xe::accel_bias_D) = -1;
A(Xe::gyro_bias_N, Xe::gyro_bias_E) = -x6;
A(Xe::gyro_bias_N, Xe::gyro_bias_D) = x7;
A(Xe::gyro_bias_E, Xe::gyro_bias_N) = x6;
A(Xe::gyro_bias_E, Xe::gyro_bias_D) = -x8;
A(Xe::gyro_bias_D, Xe::gyro_bias_N) = -x7;
A(Xe::gyro_bias_D, Xe::gyro_bias_E) = x8;
A(Xe::pos_N, Xe::vel_N) = 1;
A(Xe::pos_E, Xe::vel_E) = 1;
A(Xe::asl, Xe::vel_D) = -1;

In [15]:

    

wind_i = wind_N*frame_i.x + wind_E*frame_i.y + wind_D*frame_i.z
vel_i = vel_N*frame_i.x + vel_E*frame_i.y + vel_D*frame_i.z


    

In [16]:

    

rel_wind = wind_i - vel_i
y_airspeed = sympy.Matrix([rel_wind.dot(-frame_b.x)]).subs(sub_C_nb)
y_airspeed


    

    
Out[16]:





$$\left[\begin{matrix}\left(- vel_{D} + wind_{D}\right) \left(- \left(2 rot_{D} rot_{E} + 2 rot_{N}\right) C_{nb(1, 0)} - \left(2 rot_{D} rot_{N} - 2 rot_{E}\right) C_{nb(0, 0)} - \left(rot^{2}_{D} - rot^{2}_{E} - rot^{2}_{N} + 1\right) C_{nb(2, 0)}\right) + \left(- vel_{E} + wind_{E}\right) \left(- \left(2 rot_{D} rot_{E} - 2 rot_{N}\right) C_{nb(2, 0)} - \left(2 rot_{D} + 2 rot_{E} rot_{N}\right) C_{nb(0, 0)} - \left(- rot^{2}_{D} + rot^{2}_{E} - rot^{2}_{N} + 1\right) C_{nb(1, 0)}\right) + \left(- vel_{N} + wind_{N}\right) \left(- \left(2 rot_{D} rot_{N} + 2 rot_{E}\right) C_{nb(2, 0)} - \left(- 2 rot_{D} + 2 rot_{E} rot_{N}\right) C_{nb(1, 0)} - \left(- rot^{2}_{D} - rot^{2}_{E} + rot^{2}_{N} + 1\right) C_{nb(0, 0)}\right)\end{matrix}\right]$$

In [17]:

    

H_airspeed = y_airspeed.jacobian(xe).subs(sub_lin)
H_airspeed.T


    

    
Out[17]:





$$\left[\begin{matrix}- 2 \left(- vel_{D} + wind_{D}\right) C_{nb(1, 0)} + 2 \left(- vel_{E} + wind_{E}\right) C_{nb(2, 0)}\\2 \left(- vel_{D} + wind_{D}\right) C_{nb(0, 0)} - 2 \left(- vel_{N} + wind_{N}\right) C_{nb(2, 0)}\\- 2 \left(- vel_{E} + wind_{E}\right) C_{nb(0, 0)} + 2 \left(- vel_{N} + wind_{N}\right) C_{nb(1, 0)}\\C_{nb(0, 0)}\\C_{nb(1, 0)}\\C_{nb(2, 0)}\\0\\0\\0\\0\\0\\0\\0\\0\\0\\0\\0\\- C_{nb(0, 0)}\\- C_{nb(1, 0)}\\- C_{nb(2, 0)}\end{matrix}\right]$$

In [18]:

    

matrix_to_code('H', H_airspeed,
               'Y_airspeed::', [sympy.Symbol('airspeed')],
               'Xe::', xe)


    

    




Matrix<float, Y_airspeed::n, Xe::n> H;
float x0 = C_nb(1, 0);
float x1 = 2*vel_D - 2*wind_D;
float x2 = C_nb(2, 0);
float x3 = 2*vel_E - 2*wind_E;
float x4 = C_nb(0, 0);
float x5 = 2*vel_N - 2*wind_N;
H(Y_airspeed::airspeed, Xe::rot_N) = x0*x1 - x2*x3;
H(Y_airspeed::airspeed, Xe::rot_E) = -x1*x4 + x2*x5;
H(Y_airspeed::airspeed, Xe::rot_D) = -x0*x5 + x3*x4;
H(Y_airspeed::airspeed, Xe::vel_N) = x4;
H(Y_airspeed::airspeed, Xe::vel_E) = x0;
H(Y_airspeed::airspeed, Xe::vel_D) = x2;
H(Y_airspeed::airspeed, Xe::wind_N) = -x4;
H(Y_airspeed::airspeed, Xe::wind_E) = -x0;
H(Y_airspeed::airspeed, Xe::wind_D) = -x2;

In [19]:

    

d_eq = sympy.solve((d*frame_b.z).dot(frame_i.z).subs(sub_C_nb) - (asl - terrain_asl), d)[0]
d_eq.subs(sub_agl)


    

    
Out[19]:





$$\frac{agl}{2 C_{nb(0, 2)} rot_{D} rot_{N} - 2 C_{nb(0, 2)} rot_{E} + 2 C_{nb(1, 2)} rot_{D} rot_{E} + 2 C_{nb(1, 2)} rot_{N} + C_{nb(2, 2)} rot^{2}_{D} - C_{nb(2, 2)} rot^{2}_{E} - C_{nb(2, 2)} rot^{2}_{N} + C_{nb(2, 2)}}$$

In [20]:

    

y_dist = sympy.Matrix([d_eq]).subs(sub_C_nb)
y_dist[0].subs(sub_lin).subs(sub_agl)


    

    
Out[20]:





$$\frac{agl}{C_{nb(2, 2)}}$$

In [21]:

    

H_distance = y_dist.jacobian(xe).subs(sub_lin).subs(sub_agl)
H_distance.T
matrix_to_code('H', H_distance, 'Y_distance_down::',
               [sympy.symbols('d')], 'Xe::', xe)


    

    




Matrix<float, Y_distance_down::n, Xe::n> H;
float x0 = C_nb(2, 2);
float x1 = 2*agl/x0**2;
float x2 = 1/x0;
H(Y_distance_down::d, Xe::rot_N) = -x1*C_nb(1, 2);
H(Y_distance_down::d, Xe::rot_E) = x1*C_nb(0, 2);
H(Y_distance_down::d, Xe::asl) = x2;
H(Y_distance_down::d, Xe::terrain_asl) = -x2;

In [22]:

    

#omega_nx, omega_ny, omega_nz = sympy.symbols('\omega_{nx}, \omega_{ny}, \omega_{nz}')
#omega_ib_n = omega_nx*frame_i.x + omega_ny*frame_i.y + omega_nz*frame_i.z
#omega_ib_n


    

In [23]:

    

y_flow_sym = [flowX, flowY]
omega_n = (omega_ib_b - gyro_bias_i)
vel_f_b = -vel_i - omega_n.cross(d_eq*frame_b.z)
vel_f_b.subs(sub_lin).subs(sub_agl)


    

    
Out[23]:





$$-  vel_{N}\mathbf{\hat{i}_x} -  vel_{E}\mathbf{\hat{i}_y} -  vel_{D}\mathbf{\hat{i}_z} -  \frac{agl \omega_{by}}{C_{nb(2, 2)}}\mathbf{\hat{b}_x} + \frac{agl \omega_{bx}}{C_{nb(2, 2)}}\mathbf{\hat{b}_y}$$

In [24]:

    

y_flow = sympy.Matrix([
    -vel_f_b.dot(frame_b.x).subs(sub_C_nb),
    -vel_f_b.dot(frame_b.y).subs(sub_C_nb)
    ]).subs(sub_C_nb)


    

In [25]:

    

def sym2latex(s):
    return sympy.latex(s).replace(r'{\left (t \right )}', '')


    

In [26]:

    

y_flow_lin = y_flow.subs(sub_lin).subs(sub_agl).subs(sub_C_nb)
y_flow_lin.simplify()
matrix_to_code('y_flow_lin', y_flow_lin, 'Y_flow::', y_flow_sym, '', [0])


    

    




Matrix<float, Y_flow::n, n> y_flow_lin;
float x0 = vel_N;
float x1 = vel_E;
float x2 = vel_D;
float x3 = agl/C_nb(2, 2);
y_flow_lin(Y_flow::flowX, 0) = x0*C_nb(0, 0) + x1*C_nb(1, 0) + x2*C_nb(2, 0) + x3*omega_by;
y_flow_lin(Y_flow::flowY, 0) = x0*C_nb(0, 1) + x1*C_nb(1, 1) + x2*C_nb(2, 1) - x3*omega_bx;

In [27]:

    

H_flow = y_flow.jacobian(xe).subs(sub_lin).subs(sub_agl)
H_flow


for i in range(H_flow.shape[0]):
    for j in range(H_flow.shape[1]):
        if H_flow[i, j] != 0:
            s_mat = sym2latex(H_flow[i, j])
            print('H[{:s}, {:s}] =& {:s} \\\\'.format(
                sym2latex(y_flow_sym[i]),
                sym2latex(xe[j]),
                sym2latex(H_flow[i, j])))


    

    




H[\operatorname{flowX}, \operatorname{rot_{N}}] =& 2 \operatorname{C_{nb(1, 0)}} \operatorname{vel_{D}} - \frac{2 \operatorname{C_{nb(1, 2)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}^{2}} \omega_{by} - 2 \operatorname{C_{nb(2, 0)}} \operatorname{vel_{E}} \\
H[\operatorname{flowX}, \operatorname{rot_{E}}] =& - 2 \operatorname{C_{nb(0, 0)}} \operatorname{vel_{D}} + \frac{2 \operatorname{C_{nb(0, 2)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}^{2}} \omega_{by} + 2 \operatorname{C_{nb(2, 0)}} \operatorname{vel_{N}} \\
H[\operatorname{flowX}, \operatorname{rot_{D}}] =& 2 \operatorname{C_{nb(0, 0)}} \operatorname{vel_{E}} - 2 \operatorname{C_{nb(1, 0)}} \operatorname{vel_{N}} \\
H[\operatorname{flowX}, \operatorname{vel_{N}}] =& \operatorname{C_{nb(0, 0)}} \\
H[\operatorname{flowX}, \operatorname{vel_{E}}] =& \operatorname{C_{nb(1, 0)}} \\
H[\operatorname{flowX}, \operatorname{vel_{D}}] =& \operatorname{C_{nb(2, 0)}} \\
H[\operatorname{flowX}, \operatorname{gyro_{bias N}}] =& - \frac{\operatorname{C_{nb(0, 1)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowX}, \operatorname{gyro_{bias E}}] =& - \frac{\operatorname{C_{nb(1, 1)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowX}, \operatorname{gyro_{bias D}}] =& - \frac{\operatorname{C_{nb(2, 1)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowX}, \operatorname{asl}] =& \frac{\omega_{by}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowX}, \operatorname{terrain_{asl}}] =& - \frac{\omega_{by}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowY}, \operatorname{rot_{N}}] =& 2 \operatorname{C_{nb(1, 1)}} \operatorname{vel_{D}} + \frac{2 \operatorname{C_{nb(1, 2)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}^{2}} \omega_{bx} - 2 \operatorname{C_{nb(2, 1)}} \operatorname{vel_{E}} \\
H[\operatorname{flowY}, \operatorname{rot_{E}}] =& - 2 \operatorname{C_{nb(0, 1)}} \operatorname{vel_{D}} - \frac{2 \operatorname{C_{nb(0, 2)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}^{2}} \omega_{bx} + 2 \operatorname{C_{nb(2, 1)}} \operatorname{vel_{N}} \\
H[\operatorname{flowY}, \operatorname{rot_{D}}] =& 2 \operatorname{C_{nb(0, 1)}} \operatorname{vel_{E}} - 2 \operatorname{C_{nb(1, 1)}} \operatorname{vel_{N}} \\
H[\operatorname{flowY}, \operatorname{vel_{N}}] =& \operatorname{C_{nb(0, 1)}} \\
H[\operatorname{flowY}, \operatorname{vel_{E}}] =& \operatorname{C_{nb(1, 1)}} \\
H[\operatorname{flowY}, \operatorname{vel_{D}}] =& \operatorname{C_{nb(2, 1)}} \\
H[\operatorname{flowY}, \operatorname{gyro_{bias N}}] =& \frac{\operatorname{C_{nb(0, 0)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowY}, \operatorname{gyro_{bias E}}] =& \frac{\operatorname{C_{nb(1, 0)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowY}, \operatorname{gyro_{bias D}}] =& \frac{\operatorname{C_{nb(2, 0)}} \operatorname{agl}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowY}, \operatorname{asl}] =& - \frac{\omega_{bx}}{\operatorname{C_{nb(2, 2)}}} \\
H[\operatorname{flowY}, \operatorname{terrain_{asl}}] =& \frac{\omega_{bx}}{\operatorname{C_{nb(2, 2)}}} \\

In [28]:

    

H_flow.shape[0]


    

    
Out[28]:





$$2$$

In [29]:

    

H_flow.subs(sub_C_nb_rev).subs({phi: 0, theta:0, psi:0})


    

    
Out[29]:





$$\left[\begin{array}{cccccccccccccccccccc}0 & - 2 vel_{D} & 2 vel_{E} & 1 & 0 & 0 & 0 & - agl & 0 & 0 & 0 & 0 & 0 & 0 & \omega_{by} & - \omega_{by} & 0 & 0 & 0 & 0\\2 vel_{D} & 0 & - 2 vel_{N} & 0 & 1 & 0 & agl & 0 & 0 & 0 & 0 & 0 & 0 & 0 & - \omega_{bx} & \omega_{bx} & 0 & 0 & 0 & 0\end{array}\right]$$

In [30]:

    

P = sympy.diag(*[sympy.Symbol('var_' + str(xi)) for xi in xe])
R = sympy.diag(*[sympy.Symbol('var_flowY'), sympy.Symbol('var_flowX')])
#P = sympy.MatrixSymbol('P', len(xe), len(xe))
#R = sympy.MatrixSymbol('R', 2, 2)
S = H_flow * P * H_flow.T + R
S.simplify()


    

In [31]:

    

S[0, 0].subs(sub_agl)


    

    
Out[31]:





$$\frac{1}{C^{4}_{nb(2, 2)}} \left(4 var_{rot E(t)} \left(\left(- C_{nb(0, 0)} vel_{D} + C_{nb(2, 0)} vel_{N}\right) C^{2}_{nb(2, 2)} + C_{nb(0, 2)} agl \omega_{by}\right)^{2} + 4 var_{rot N(t)} \left(\left(- C_{nb(1, 0)} vel_{D} + C_{nb(2, 0)} vel_{E}\right) C^{2}_{nb(2, 2)} + C_{nb(1, 2)} agl \omega_{by}\right)^{2} + \left(var_{flowY} + 4 var_{rot D(t)} \left(C_{nb(0, 0)} vel_{E} - C_{nb(1, 0)} vel_{N}\right)^{2} + var_{vel D(t)} C^{2}_{nb(2, 0)} + var_{vel E(t)} C^{2}_{nb(1, 0)} + var_{vel N(t)} C^{2}_{nb(0, 0)}\right) C^{4}_{nb(2, 2)} + \left(var_{asl(t)} \omega^{2}_{by} + var_{gyro bias D(t)} C^{2}_{nb(2, 1)} agl^{2} + var_{gyro bias E(t)} C^{2}_{nb(1, 1)} agl^{2} + var_{gyro bias N(t)} C^{2}_{nb(0, 1)} agl^{2} + var_{terrain asl(t)} \omega^{2}_{by}\right) C^{2}_{nb(2, 2)}\right)$$

In [32]:

    

S[1, 1].subs(sub_agl)


    

    
Out[32]:





$$\frac{1}{C^{4}_{nb(2, 2)}} \left(4 var_{rot E(t)} \left(\left(C_{nb(0, 1)} vel_{D} - C_{nb(2, 1)} vel_{N}\right) C^{2}_{nb(2, 2)} + C_{nb(0, 2)} agl \omega_{bx}\right)^{2} + 4 var_{rot N(t)} \left(\left(C_{nb(1, 1)} vel_{D} - C_{nb(2, 1)} vel_{E}\right) C^{2}_{nb(2, 2)} + C_{nb(1, 2)} agl \omega_{bx}\right)^{2} + \left(var_{flowX} + 4 var_{rot D(t)} \left(C_{nb(0, 1)} vel_{E} - C_{nb(1, 1)} vel_{N}\right)^{2} + var_{vel D(t)} C^{2}_{nb(2, 1)} + var_{vel E(t)} C^{2}_{nb(1, 1)} + var_{vel N(t)} C^{2}_{nb(0, 1)}\right) C^{4}_{nb(2, 2)} + \left(var_{asl(t)} \omega^{2}_{bx} + var_{gyro bias D(t)} C^{2}_{nb(2, 0)} agl^{2} + var_{gyro bias E(t)} C^{2}_{nb(1, 0)} agl^{2} + var_{gyro bias N(t)} C^{2}_{nb(0, 0)} agl^{2} + var_{terrain asl(t)} \omega^{2}_{bx}\right) C^{2}_{nb(2, 2)}\right)$$

In [33]:

    

S[0, 0].subs(sub_agl).subs(sub_C_nb_rev).subs({phi: 0, theta: 0})


    

    
Out[33]:





$$var_{asl(t)} \omega^{2}_{by} + var_{flowY} + var_{gyro bias E(t)} agl^{2} \operatorname{cos}^{2}\left(\psi\right) + var_{gyro bias N(t)} agl^{2} \operatorname{sin}^{2}\left(\psi\right) + 4 var_{rot D(t)} \left(vel_{E} \operatorname{cos}\left(\psi\right) - vel_{N} \operatorname{sin}\left(\psi\right)\right)^{2} + 4 var_{rot E(t)} vel^{2}_{D} \operatorname{cos}^{2}\left(\psi\right) + 4 var_{rot N(t)} vel^{2}_{D} \operatorname{sin}^{2}\left(\psi\right) + var_{terrain asl(t)} \omega^{2}_{by} + var_{vel E(t)} \operatorname{sin}^{2}\left(\psi\right) + var_{vel N(t)} \operatorname{cos}^{2}\left(\psi\right)$$

In [34]:

    

S[1, 1].subs(sub_agl).subs(sub_C_nb_rev).subs({phi: 0, theta: 0, psi:0})


    

    
Out[34]:





$$var_{asl(t)} \omega^{2}_{bx} + var_{flowX} + var_{gyro bias N(t)} agl^{2} + 4 var_{rot D(t)} vel^{2}_{N} + 4 var_{rot N(t)} vel^{2}_{D} + var_{terrain asl(t)} \omega^{2}_{bx} + var_{vel E(t)}$$

In [35]:

    

S.subs(sub_agl).subs(sub_C_nb_rev).subs({phi: 0, theta: 0, psi:0, omega_bx:0, omega_by: 0})


    

    
Out[35]:





$$\left[\begin{matrix}var_{flowY} + var_{gyro bias E(t)} agl^{2} + 4 var_{rot D(t)} vel^{2}_{E} + 4 var_{rot E(t)} vel^{2}_{D} + var_{vel N(t)} & - 4 var_{rot D(t)} vel_{E} vel_{N}\\- 4 var_{rot D(t)} vel_{E} vel_{N} & var_{flowX} + var_{gyro bias N(t)} agl^{2} + 4 var_{rot D(t)} vel^{2}_{N} + 4 var_{rot N(t)} vel^{2}_{D} + var_{vel E(t)}\end{matrix}\right]$$

In [36]:

    

H_flow.subs(sub_agl).subs(sub_C_nb_rev).subs({phi: 0, theta: 0, psi:0, omega_bx:0, omega_by: 0})


    

    
Out[36]:





$$\left[\begin{array}{cccccccccccccccccccc}0 & - 2 vel_{D} & 2 vel_{E} & 1 & 0 & 0 & 0 & - agl & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\2 vel_{D} & 0 & - 2 vel_{N} & 0 & 1 & 0 & agl & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$

In [37]:

    

matrix_to_code('S', sympy.diag(S[0,0]), 'Y_flow::', y_flow_sym, 'Y_flow::', y_flow_sym,)


    

    




Matrix<float, Y_flow::n, Y_flow::n> S;
float x0 = C_nb(2, 2);
float x1 = x0**4;
float x2 = agl;
float x3 = omega_by;
float x4 = x2*x3;
float x5 = x0**2;
float x6 = C_nb(0, 0);
float x7 = vel_D;
float x8 = C_nb(2, 0);
float x9 = vel_N;
float x10 = C_nb(1, 0);
float x11 = vel_E;
float x12 = x3**2;
float x13 = x2**2;
S(Y_flow::flowX, Y_flow::flowX) = (4*var_rot_E*(-x4*C_nb(0, 2) + x5*(x6*x7 - x8*x9))**2 + 4*var_rot_N*(-x4*C_nb(1, 2) + x5*(x10*x7 - x11*x8))**2 + x1*(var_flowY + 4*var_rot_D*(-x10*x9 + x11*x6)**2 + var_vel_D*x8**2 + var_vel_E*x10**2 + var_vel_N*x6**2) + x5*(var_asl*x12 + var_gyro_bias_D*x13*C_nb(2, 1)**2 + var_gyro_bias_E*x13*C_nb(1, 1)**2 + var_gyro_bias_N*x13*C_nb(0, 1)**2 + var_terrain_asl*x12))/x1;

In [38]:

    

matrix_to_code('H', H_flow, 'Y_flow::', y_flow_sym, 'Xe::', xe)


    

    




Matrix<float, Y_flow::n, Xe::n> H;
float x0 = vel_D;
float x1 = C_nb(1, 0);
float x2 = 2*x1;
float x3 = vel_E;
float x4 = C_nb(2, 0);
float x5 = 2*x4;
float x6 = omega_by;
float x7 = agl;
float x8 = C_nb(2, 2);
float x9 = x8**(-2);
float x10 = 2*x7*x9*C_nb(1, 2);
float x11 = C_nb(0, 0);
float x12 = 2*x11;
float x13 = vel_N;
float x14 = 2*x7*x9*C_nb(0, 2);
float x15 = C_nb(0, 1);
float x16 = 1/x8;
float x17 = x16*x7;
float x18 = C_nb(1, 1);
float x19 = C_nb(2, 1);
float x20 = x16*x6;
float x21 = 2*x18;
float x22 = 2*x19;
float x23 = omega_bx;
float x24 = 2*x15;
float x25 = x16*x23;
H(Y_flow::flowX, Xe::rot_N) = x0*x2 - x10*x6 - x3*x5;
H(Y_flow::flowX, Xe::rot_E) = -x0*x12 + x13*x5 + x14*x6;
H(Y_flow::flowX, Xe::rot_D) = x12*x3 - x13*x2;
H(Y_flow::flowX, Xe::vel_N) = x11;
H(Y_flow::flowX, Xe::vel_E) = x1;
H(Y_flow::flowX, Xe::vel_D) = x4;
H(Y_flow::flowX, Xe::gyro_bias_N) = -x15*x17;
H(Y_flow::flowX, Xe::gyro_bias_E) = -x17*x18;
H(Y_flow::flowX, Xe::gyro_bias_D) = -x17*x19;
H(Y_flow::flowX, Xe::asl) = x20;
H(Y_flow::flowX, Xe::terrain_asl) = -x20;
H(Y_flow::flowY, Xe::rot_N) = x0*x21 + x10*x23 - x22*x3;
H(Y_flow::flowY, Xe::rot_E) = -x0*x24 + x13*x22 - x14*x23;
H(Y_flow::flowY, Xe::rot_D) = -x13*x21 + x24*x3;
H(Y_flow::flowY, Xe::vel_N) = x15;
H(Y_flow::flowY, Xe::vel_E) = x18;
H(Y_flow::flowY, Xe::vel_D) = x19;
H(Y_flow::flowY, Xe::gyro_bias_N) = x11*x17;
H(Y_flow::flowY, Xe::gyro_bias_E) = x1*x17;
H(Y_flow::flowY, Xe::gyro_bias_D) = x17*x4;
H(Y_flow::flowY, Xe::asl) = -x25;
H(Y_flow::flowY, Xe::terrain_asl) = x25;

In [39]:

    

y_attitude = sympy.Matrix([
        rot_N, rot_E, rot_D
    ])
H_attitude = y_attitude.jacobian(xe).subs(sub_lin).subs(sub_agl)
H_attitude


    

    
Out[39]:





$$\left[\begin{array}{cccccccccccccccccccc}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$

In [40]:

    

g = sympy.symbols('g')
g_i = -g*frame_i.z + accel_bias_N*frame_i.x + accel_bias_E*frame_i.y + accel_bias_D*frame_i.z
y_accel = sympy.Matrix(g_i.express(frame_b).subs(sub_C_nb).to_matrix(frame_b))
H_accel = y_accel.jacobian(xe).subs(sub_lin)
H_accel


    

    
Out[40]:





$$\left[\begin{array}{cccccccccccccccccccc}2 \left(- g + accel_{biasD}\right) C_{nb(1, 0)} - 2 C_{nb(2, 0)} accel_{biasE} & - 2 \left(- g + accel_{biasD}\right) C_{nb(0, 0)} + 2 C_{nb(2, 0)} accel_{biasN} & 2 C_{nb(0, 0)} accel_{biasE} - 2 C_{nb(1, 0)} accel_{biasN} & 0 & 0 & 0 & 0 & 0 & 0 & C_{nb(0, 0)} & C_{nb(1, 0)} & C_{nb(2, 0)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\2 \left(- g + accel_{biasD}\right) C_{nb(1, 1)} - 2 C_{nb(2, 1)} accel_{biasE} & - 2 \left(- g + accel_{biasD}\right) C_{nb(0, 1)} + 2 C_{nb(2, 1)} accel_{biasN} & 2 C_{nb(0, 1)} accel_{biasE} - 2 C_{nb(1, 1)} accel_{biasN} & 0 & 0 & 0 & 0 & 0 & 0 & C_{nb(0, 1)} & C_{nb(1, 1)} & C_{nb(2, 1)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\2 \left(- g + accel_{biasD}\right) C_{nb(1, 2)} - 2 C_{nb(2, 2)} accel_{biasE} & - 2 \left(- g + accel_{biasD}\right) C_{nb(0, 2)} + 2 C_{nb(2, 2)} accel_{biasN} & 2 C_{nb(0, 2)} accel_{biasE} - 2 C_{nb(1, 2)} accel_{biasN} & 0 & 0 & 0 & 0 & 0 & 0 & C_{nb(0, 2)} & C_{nb(1, 2)} & C_{nb(2, 2)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$

In [41]:

    

H_accel.subs(sub_C_nb_rev).subs({phi: 0, theta: 0, psi: 0})


    

    
Out[41]:





$$\left[\begin{array}{cccccccccccccccccccc}0 & 2 g - 2 accel_{biasD} & 2 accel_{biasE} & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\- 2 g + 2 accel_{biasD} & 0 & - 2 accel_{biasN} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\- 2 accel_{biasE} & 2 accel_{biasN} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$

In [42]:

    

B_N, B_E, B_D = sympy.symbols('B_N, B_E, B_D')
b_i = B_N*frame_i.x + B_E*frame_i.y + B_D*frame_i.z
y_mag = sympy.Matrix(b_i.express(frame_b).subs(sub_C_nb).to_matrix(frame_b))
H_mag = y_mag.jacobian(xe).subs(sub_lin).subs(sub_agl)
H_mag.simplify()
H_mag


    

    
Out[42]:





$$\left[\begin{array}{cccccccccccccccccccc}2 B_{D} C_{nb(1, 0)} - 2 B_{E} C_{nb(2, 0)} & - 2 B_{D} C_{nb(0, 0)} + 2 B_{N} C_{nb(2, 0)} & 2 B_{E} C_{nb(0, 0)} - 2 B_{N} C_{nb(1, 0)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\2 B_{D} C_{nb(1, 1)} - 2 B_{E} C_{nb(2, 1)} & - 2 B_{D} C_{nb(0, 1)} + 2 B_{N} C_{nb(2, 1)} & 2 B_{E} C_{nb(0, 1)} - 2 B_{N} C_{nb(1, 1)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\2 B_{D} C_{nb(1, 2)} - 2 B_{E} C_{nb(2, 2)} & - 2 B_{D} C_{nb(0, 2)} + 2 B_{N} C_{nb(2, 2)} & 2 B_{E} C_{nb(0, 2)} - 2 B_{N} C_{nb(1, 2)} & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$$

In [43]:

    

def find_observable_states(H, x, n_max=3):
    O = sympy.Matrix(H)
    for n in range(n_max):
        O = O.col_join(H*A**n)
    return [x[i] for i in O.rref()[1]]


    

In [44]:

    

find_observable_states(H_mag, xe)


    

    
Out[44]:





$$\left [ rot_{N}, \quad rot_{E}, \quad rot_{D}, \quad gyro_{biasN}, \quad gyro_{biasE}, \quad gyro_{biasD}\right ]$$

In [45]:

    

find_observable_states(H_accel, xe)


    

    
Out[45]:





$$\left [ rot_{N}, \quad rot_{E}, \quad rot_{D}, \quad gyro_{biasN}, \quad gyro_{biasE}, \quad gyro_{biasD}\right ]$$

In [46]:

    

find_observable_states(H_mag.col_join(H_accel), xe)


    

    
Out[46]:





$$\left [ rot_{N}, \quad rot_{E}, \quad rot_{D}, \quad gyro_{biasN}, \quad gyro_{biasE}, \quad gyro_{biasD}, \quad accel_{biasN}, \quad accel_{biasE}, \quad accel_{biasD}\right ]$$